#pragma once
#include <vector>
#include <assert.h>
#include "Utility/Algebra/Matrix.h"
#include "SGFem/Pre/Assembly/CoordinateProcessor.h"

class ElementTools
{
public:
    /** @brief 获取单元节点位移列向量
     * 
     * @param dofGlobalIds 单元节点对应的全局自由度编号
     * @param [out] eleDisp 单元节点位移列向量，出于效率考虑，应提前为`eleDisp`分配正确的维度
     */
    static void GetElementDisplacement (const std::vector<int>& dofGlobalIds, const std::vector<SG::DataStructure::Common::Real>& globalDisplacement, SG::Algebra::Matrixd& eleDisp);

    /**
     * @brief    计算单元局部坐标系，并将节点坐标从Basic 变换到单元矩阵坐标系下
     * 
     *  单元局部坐标系以输出的第一个为原点， 1-2 点 为x轴矢量方向， 第3点确定x-y 平面， 次计算方法假设所有节点都位于同一个平面内
     * @param    points      单元节点Basic坐标系下坐标
     * @param[out]    T           单元坐标转换矩阵（局部到全局）
     * @param[out]    origin      坐标系原点
     * @param[out]    localCoord  节点在单元局部坐标系下坐标
     * @author   wanghuan
     * @date     2025.03.15
     */
    static void computePlaneCoordTrans (const SG::Algebra::PointArray& points, _OUT SG::Algebra::Matrix& T, SG::Algebra::Point& _OUT origin, _OUT SG::Algebra::Matrix& localCoord);

    // 新版四节点壳单元全局->局部坐标系转换 24.09.24 lxy
    static void computeQaud4CoordTrans (const SG::Algebra::PointArray& points, _OUT SG::Algebra::Matrix& T, SG::Algebra::Point& _OUT origin, _OUT SG::Algebra::Matrix& localCoord);

    /** @brief    行叠加法计算集中质量矩阵
     * 
     * @param    coupledMass 协调质量矩阵， 应保证传入方阵
     * @return   
     * @author   wanghuan
     * @date     2025.04.25
     */
    static SG::Algebra::Matrixd ComputeLumpMass (const SG::Algebra::Matrixd& coupledMass)
    {
        const auto dim = coupledMass.GetColSize ();
        SG::Algebra::Matrixd lumpMass(dim, dim);
        for (std::size_t iNode{0}; iNode < dim; ++iNode)
        {
            SG::DataStructure::Common::Real sum{0.0};
            for (std::size_t jNode{0}; jNode < dim; ++jNode)
            {
                sum += coupledMass(iNode, jNode);
            }
            lumpMass (iNode, iNode) = sum;
        }
        return lumpMass;
    }

    /** @brief    梁单元节点位移输出坐标系和 单元局部坐标系之间的转换矩阵
     * 
     * @param    T             3*3矩阵，单元局部坐标系,每行代表一个基矢量方向
     * @param    dispCoordGA   节点A的位移输出坐标系编号
     * @param    dispCoordGB   节点B的位移输出坐标系编号
     * @param    GA            节点A在世界坐标系下的坐标
     * @param    GB            节点B在世界坐标系下的坐标
     * @return                 12*12, 可以将梁刚度从单元局部坐标变换到节点的位移输出坐标系，  kg = TBeam.Transpose () * kl * TBeam;
     * @author   wanghuan
     * @date     2025.04.25
     */
    static SG::Algebra::Matrixd TwoNodeEleCoordTrans (SG::Algebra::Matrix& T, SG::DataStructure::Common::Id dispCoordGA, SG::DataStructure::Common::Id dispCoordGB, const SG::Algebra::Point& GA, const SG::Algebra::Point& GB)
    {
        auto&       coordHandler   = SG::FEMSolver::CoordinateProcessor::GetInstance ();
        SG::Algebra::Matrixd TBeam (12, 12);
        SG::Algebra::Matrix  TGi;
        auto  TA = T;
        if (dispCoordGA > 0)
        {
            coordHandler.GetCoordTrans (dispCoordGA, GA, _OUT TGi);
            TA = T * TGi.Transpose ();
        }
        TBeam.BlockFill (0, 0, TA);
        TBeam.BlockFill (3, 3, TA);

        auto  TB = T;
        if (dispCoordGB > 0)
        {
            coordHandler.GetCoordTrans (dispCoordGB, GB, _OUT TGi);
            TB = T *TGi.Transpose ();
        }
        TBeam.BlockFill (6, 6, TB);
        TBeam.BlockFill (9, 9, TB);

        return TBeam;
    };
};

inline void ElementTools::GetElementDisplacement (const std::vector<int>& dofGlobalIds, const std::vector<SG::DataStructure::Common::Real>& globalDisplacement, SG::Algebra::Matrixd& eleDisp)
{
    assert (dofGlobalIds.size () == eleDisp.GetRowSize ());
    assert (1 == eleDisp.GetColSize ());

    const auto size = dofGlobalIds.size ();
    for (std::size_t iDof{0}; iDof < size; ++iDof)
    {
        eleDisp (iDof, 0) = globalDisplacement[dofGlobalIds[iDof]];
    }
}

inline void ElementTools::computePlaneCoordTrans (const SG::Algebra::PointArray& points, _OUT SG::Algebra::Matrix& T, SG::Algebra::Point& _OUT origin, _OUT SG::Algebra::Matrix& localCoord)
{
    auto r1 = Distance (points[1], points[0]);
    SG::DataStructure::Common::Real norm;
    r1.Normalize (norm);
    auto r2 = SG::Algebra::Distance (points[2], points[0]);
    auto r3 = SG::Algebra::Cross (r1, r2);
    r3.Normalize (norm);
    r2 = SG::Algebra::Cross (r3, r1);

    T        = SG::Algebra::Matrix (3, 3);
    T (0, 0) = r1.m_x;
    T (0, 1) = r2.m_x;
    T (0, 2) = r3.m_x;
    T (1, 0) = r1.m_y;
    T (1, 1) = r2.m_y;
    T (1, 2) = r3.m_y;
    T (2, 0) = r1.m_z;
    T (2, 1) = r2.m_z;
    T (2, 2) = r3.m_z;

    // 平移到以 节点1为原点的坐标系
    auto nNode = points.size ();
    localCoord = SG::Algebra::Matrix (3, nNode);
    for (std::size_t iNode = 1; iNode < nNode; ++iNode)
    {
        localCoord (0, iNode) = points[iNode].m_x - points[0].m_x;
        localCoord (1, iNode) = points[iNode].m_y - points[0].m_y;
        localCoord (2, iNode) = points[iNode].m_z - points[0].m_z;
    }

    // 将第一个点局部坐标设置为 {0.0，0.0，0.0}
    localCoord (0, 0) = 0.0;
    localCoord (1, 0) = 0.0;
    localCoord (2, 0) = 0.0;

    auto tmp   = T.Transpose () * localCoord;
    localCoord = SG::Algebra::Matrix (nNode, 2);
    for (std::size_t iNode = 1; iNode < nNode; ++iNode)
    {
        localCoord (iNode, 0) = tmp (0, iNode);
        localCoord (iNode, 1) = tmp (1, iNode);
    }

    origin = points[0];
}

// 新版四节点壳单元全局->局部坐标系转换 24.09.24 lxy
inline void ElementTools::computeQaud4CoordTrans (const SG::Algebra::PointArray& points,  _OUT SG::Algebra::Matrix& T, SG::Algebra::Point& _OUT origin, _OUT SG::Algebra::Matrix& localCoord)
{
    // 以1-4节点的中点作为坐标系原点
    SG::Algebra::Point central{ (points[0].m_x + points[1].m_x + points[2].m_x + points[3].m_x) / 4.0, (points[0].m_y + points[1].m_y + points[2].m_y + points[3].m_y) / 4.0, (points[0].m_z + points[1].m_z + points[2].m_z + points[3].m_z) / 4.0};

    auto nNode = points.size ();
    localCoord = SG::Algebra::Matrix (nNode, 3);
    std::vector<SG::Algebra::Point> localCoordPoints;
    for (std::size_t iNode = 0; iNode < nNode; ++iNode)
    {
        // 此处localCoordPoints与localCoord本质上均按顺序存储节点坐标
        // 为了方便应用Point类中计算向量函数Distance和归一化函数Normalize并且保持computePlaneCoordTrans接口返回数据格式不变
        // 所以此处采用两种类型重复存储，后续此处可改进
        SG::Algebra::Point tmpPoint;
        tmpPoint.m_x = points[iNode].m_x - central.m_x;
        tmpPoint.m_y = points[iNode].m_y - central.m_y;
        tmpPoint.m_z = points[iNode].m_z - central.m_z;
        localCoordPoints.push_back (tmpPoint);

        localCoord (iNode, 0) = tmpPoint.m_x;
        localCoord (iNode, 1) = tmpPoint.m_y;
        localCoord (iNode, 2) = tmpPoint.m_z;
    }
    
    SG::DataStructure::Common::Real norm;
    auto r1 = SG::Algebra::Distance (localCoordPoints[2], localCoordPoints[0]);
    auto r2 = SG::Algebra::Distance (localCoordPoints[3], localCoordPoints[1]);
    r1.Normalize (norm);
    r2.Normalize (norm);

    auto Vs = r1 - r2;
    Vs.Normalize (norm);

    auto Vz = SG::Algebra::Cross (Vs, r2);
    Vz.Normalize (norm);

    auto Vt = Cross (Vz, Vs);
    Vt.Normalize (norm);

    T        = SG::Algebra::Matrix (3, 3);
    T (0, 0) = Vs.m_x;
    T (0, 1) = Vt.m_x;
    T (0, 2) = Vz.m_x;
    T (1, 0) = Vs.m_y;
    T (1, 1) = Vt.m_y;
    T (1, 2) = Vz.m_y;
    T (2, 0) = Vs.m_z;
    T (2, 1) = Vt.m_z;
    T (2, 2) = Vz.m_z;

    // 变换到单元局部坐标系
    auto T32   = SG::Algebra::Matrix (3, 2);
    T32 (0, 0) = Vs.m_x;
    T32 (0, 1) = Vt.m_x;
    T32 (1, 0) = Vs.m_y;
    T32 (1, 1) = Vt.m_y;
    T32 (2, 0) = Vs.m_z;
    T32 (2, 1) = Vt.m_z;

    localCoord = localCoord * T32;

    origin =  SG::Algebra::Point{ (points[0].m_x + points[1].m_x + points[2].m_x + points[3].m_x) * 0.25,
        (points[0].m_y + points[1].m_y + points[2].m_y + points[3].m_y) * 0.25,
        (points[0].m_z + points[1].m_z + points[2].m_z + points[3].m_z) * 0.25 };
}